cmake_minimum_required(VERSION 3.1)
project(lio)

# add_compile_options(-std=c++14)
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_BUILD_TYPE "Release")
#set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")

find_package(catkin REQUIRED COMPONENTS
        dynamic_reconfigure
        geometry_msgs
        message_generation
        message_runtime
        nav_msgs
        pcl_conversions
        pcl_ros
        rosbag
        roscpp
        rospy
        sensor_msgs
        std_msgs
        tf
        tf_conversions
        )

list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake")
find_package(Gflags)
find_package(Glog)
include_directories(${GFLAGS_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS})

find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})

find_package(PCL REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIR})

find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIR})


catkin_package(
        #  INCLUDE_DIRS include
        #  LIBRARIES lio
        #  CATKIN_DEPENDS dynamic_reconfigure geometry_msgs message_generation message_runtime nav_msgs pcl_conversions pcl_ros rosbag roscpp rospy sensor_msgs std_msgs tf tf_conversions
        #  DEPENDS system_lib
)

include_directories(
        include
        ${catkin_INCLUDE_DIRS}
)


set(lio_EXT_LIBS
        ${PCL_LIBRARIES}
        ${OpenCV_LIBRARIES}
        ${GFLAGS_LIBRARIES}
        ${GLOG_LIBRARIES}
        )

set(point_processor_EXT_LIBS
        ${catkin_LIBRARIES}
        ${lio_EXT_LIBS}
        )

add_library(${PROJECT_NAME}-point-processor
        src/point_processor/PointMapping.cc
        src/point_processor/PointOdometry.cc
        src/point_processor/PointProcessor.cc
        )
# target_link_libraries(${PROJECT_NAME}-point-processor
#   ${point_processor_EXT_LIBS}
# )

add_library(${PROJECT_NAME}-visualizer
        src/visualizer/Visualizer.cc
        )
# target_link_libraries(${PROJECT_NAME}-visualizer
#   ${point_processor_EXT_LIBS}
# )

add_library(${PROJECT_NAME}-feature-manager
        src/feature_manager/FeatureManager.cc
        )
# target_link_libraries(${PROJECT_NAME}-feature-manager
#   ${point_processor_EXT_LIBS}
# )

set(factor_EXT_LIBS
        ${PROJECT_NAME}-feature-manager
        ${catkin_LIBRARIES}
        ${lio_EXT_LIBS}
        ceres
        )

add_library(${PROJECT_NAME}-factor
        src/factor/PointDistanceFactor.cc
        src/factor/PoseLocalParameterization.cc
        src/factor/GravityLocalParameterization.cc
        src/factor/PlaneProjectionFactor.cc
        src/factor/PlaneToPlaneFactor.cc
        src/factor/PivotPointPlaneFactor.cc
        src/factor/MarginalizationFactor.cc
        src/factor/PriorFactor.cc
        )

add_dependencies(${PROJECT_NAME}-factor
        ${PROJECT_NAME}-feature-manager
        )

# target_link_libraries(${PROJECT_NAME}-factor
#   ${PROJECT_NAME}-feature-manager
#   ${factor_EXT_LIBS}
# )

set(imu_processor_EXT_LIBS
        ${factor_EXT_LIBS}
        )

add_library(${PROJECT_NAME}-imu-processor
        src/imu_processor/Estimator.cc
        src/imu_processor/ImuInitializer.cc
        src/imu_processor/MeasurementManager.cc
        )

add_executable(${PROJECT_NAME}_processor_node src/processor_node.cc)
add_dependencies(${PROJECT_NAME}_processor_node ${PROJECT_NAME}-point-processor)
target_link_libraries(${PROJECT_NAME}_processor_node
        ${PROJECT_NAME}-point-processor
        ${point_processor_EXT_LIBS}
        )

add_executable(${PROJECT_NAME}_odometry_node src/odometry_node.cc)
add_dependencies(${PROJECT_NAME}_odometry_node ${PROJECT_NAME}-point-processor)
target_link_libraries(${PROJECT_NAME}_odometry_node
        ${PROJECT_NAME}-point-processor
        ${point_processor_EXT_LIBS}
        )

add_executable(${PROJECT_NAME}_mapping_node src/mapping_node.cc)
add_dependencies(${PROJECT_NAME}_mapping_node ${PROJECT_NAME}-point-processor)
target_link_libraries(${PROJECT_NAME}_mapping_node
        ${PROJECT_NAME}-point-processor
        ${point_processor_EXT_LIBS}
        )

add_executable(${PROJECT_NAME}_estimator_node src/estimator_node.cc)
add_dependencies(${PROJECT_NAME}_estimator_node
        ${PROJECT_NAME}-imu-processor
        ${PROJECT_NAME}-factor
        ${PROJECT_NAME}-feature-manager
        ${PROJECT_NAME}-point-processor
        ${PROJECT_NAME}-visualizer
        )
target_link_libraries(${PROJECT_NAME}_estimator_node
        ${PROJECT_NAME}-imu-processor
        ${PROJECT_NAME}-factor
        ${PROJECT_NAME}-feature-manager
        ${PROJECT_NAME}-point-processor
        ${PROJECT_NAME}-visualizer
        ${imu_processor_EXT_LIBS}
        )

add_executable(${PROJECT_NAME}_input_filters_node src/input_filters_node.cc)
target_link_libraries(${PROJECT_NAME}_input_filters_node
        ${imu_processor_EXT_LIBS}
        )

add_library(${PROJECT_NAME}-map-builder src/map_builder/MapBuilder.cc)
# add_dependencies(${PROJECT_NAME}-map-builder
#   ${PROJECT_NAME}-point-processor
# )
# target_link_libraries(${PROJECT_NAME}-map-builder
#   ${PROJECT_NAME}-point-processor
# )

add_executable(${PROJECT_NAME}_map_builder_node src/map_builder_node.cc)
add_dependencies(${PROJECT_NAME}_map_builder_node
        ${PROJECT_NAME}-map-builder
        )
target_link_libraries(${PROJECT_NAME}_map_builder_node
        ${PROJECT_NAME}-map-builder
        ${PROJECT_NAME}-point-processor
        ${point_processor_EXT_LIBS}
        )

add_executable(${PROJECT_NAME}_save_bag_to_pcd src/save_bag_to_pcd.cc)
add_dependencies(${PROJECT_NAME}_save_bag_to_pcd
        ${PROJECT_NAME}-map-builder
        )
target_link_libraries(${PROJECT_NAME}_save_bag_to_pcd
        ${point_processor_EXT_LIBS}
        )

catkin_add_gtest(${PROJECT_NAME}-test-point-processor test/test_point_processor/test_point_processor.cc)
add_dependencies(${PROJECT_NAME}-test-point-processor ${PROJECT_NAME}-point-processor)
target_link_libraries(${PROJECT_NAME}-test-point-processor
        ${PROJECT_NAME}-point-processor
        ${point_processor_EXT_LIBS}
        )

catkin_add_gtest(${PROJECT_NAME}-test-point-odometry test/test_point_processor/test_point_odometry.cc)
add_dependencies(${PROJECT_NAME}-test-point-odometry ${PROJECT_NAME}-point-processor)
target_link_libraries(${PROJECT_NAME}-test-point-odometry
        ${PROJECT_NAME}-point-processor
        ${point_processor_EXT_LIBS}
        )

catkin_add_gtest(${PROJECT_NAME}-test-point-mapping test/test_point_processor/test_point_mapping.cc)
add_dependencies(${PROJECT_NAME}-test-point-mapping ${PROJECT_NAME}-point-processor)
target_link_libraries(${PROJECT_NAME}-test-point-mapping
        ${PROJECT_NAME}-point-processor
        ${point_processor_EXT_LIBS}
        )

catkin_add_gtest(${PROJECT_NAME}-test-rotations test/test_rotations.cc)
add_dependencies(${PROJECT_NAME}-test-rotations ${PROJECT_NAME}-point-processor)
target_link_libraries(${PROJECT_NAME}-test-rotations
        ${PROJECT_NAME}-point-processor
        ${point_processor_EXT_LIBS}
        )

catkin_add_gtest(${PROJECT_NAME}-test-measurement-manager test/test_imu_processor/test_measurement_manager.cc)
add_dependencies(${PROJECT_NAME}-test-measurement-manager
        ${PROJECT_NAME}-imu-processor
        ${PROJECT_NAME}-feature-manager
        ${PROJECT_NAME}-visualizer
        )
target_link_libraries(${PROJECT_NAME}-test-measurement-manager
        ${PROJECT_NAME}-imu-processor
        ${PROJECT_NAME}-feature-manager
        ${PROJECT_NAME}-visualizer
        ${imu_processor_EXT_LIBS}
        )

catkin_add_gtest(${PROJECT_NAME}-test-circular-buffer test/test_imu_processor/test_circular_buffer.cc)
add_dependencies(${PROJECT_NAME}-test-circular-buffer ${PROJECT_NAME}-imu-processor)
target_link_libraries(${PROJECT_NAME}-test-circular-buffer
        ${PROJECT_NAME}-imu-processor
        ${imu_processor_EXT_LIBS}
        )

catkin_add_gtest(${PROJECT_NAME}-test-visualizer test/test_visualizaer.cc)
add_dependencies(${PROJECT_NAME}-test-visualizer ${PROJECT_NAME}-visualizer)
target_link_libraries(
        ${PROJECT_NAME}-test-visualizer
        ${PROJECT_NAME}-visualizer
        )

catkin_add_gtest(${PROJECT_NAME}-test-imu-factor test/test_imu_processor/test_imu_factor.cc)
add_dependencies(
        ${PROJECT_NAME}-test-imu-factor
        #  ${PROJECT_NAME}-imu-processor
        #  ${PROJECT_NAME}-feature-manager
        #  ${PROJECT_NAME}-visualizer
        ${PROJECT_NAME}-factor
        )
target_link_libraries(
        ${PROJECT_NAME}-test-imu-factor
        #  ${PROJECT_NAME}-imu-processor
        #  ${imu_processor_EXT_LIBS}
        #  ${PROJECT_NAME}-feature-manager
        #  ${PROJECT_NAME}-visualizer
        ${PROJECT_NAME}-factor
        )
